/***************************************************************************
*   Copyright (C) 2010-2011 by swkyer <swkyer@gmail.com>                  *
*                                                                         *
*   This program is free software; you can redistribute it and/or modify  *
*   it under the terms of the GNU General Public License as published by  *
*   the Free Software Foundation; either version 2 of the License, or     *
*   (at your option) any later version.                                   *
*                                                                         *
*   This program is distributed in the hope that it will be useful,       *
*   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
*   GNU General Public License for more details.                          *
*                                                                         *
*   You should have received a copy of the GNU General Public License     *
*   along with this program; if not, write to the                         *
*   Free Software Foundation, Inc.,                                       *
*   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
***************************************************************************/
#include "stdafx.h"
#include "JlinkProtocol.h"


static int jlinkusb_simple_command(jlink_usb_t *devh, uint8_t command)
{
	int retv;

	if (devh == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = command;
	retv = jlinkusb_write(devh, devh->out_buffer, 1);
	jlinkusb_unlock(devh);

	return retv==1?0:-2;
}


JLINKUSB_API int jlinkusb_get_version(jlink_usb_t *devh, char version[0x100])
{
	int retv, recvlen;

	if (devh == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_VERSION;
	recvlen = 2 + 0x70;
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		version[0] = 0;
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	memcpy(version, &devh->in_buffer[2], recvlen-2);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_get_speed(jlink_usb_t *devh, jlink_speed_t *pspeed)
{
	int retv, recvlen;

	if (devh == NULL || pspeed == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_GET_SPEEDS;
	recvlen = 6;
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}
	pspeed->BaseFreq = buf_get_u32(devh->in_buffer);
	pspeed->MinDiv = buf_get_u16(&devh->in_buffer[4]);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_get_max_mem_block(jlink_usb_t *devh, uint32_t *pmaxblock)
{
	int retv, recvlen;

	if (devh == NULL || pmaxblock == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_GET_MAX_MEM_BLOCK;
	recvlen = sizeof(uint32_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	*pmaxblock = buf_get_u32(devh->in_buffer);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_get_caps(jlink_usb_t *devh, jlink_caps_t *pcaps)
{
	int retv, recvlen;

	if (devh == NULL || pcaps == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_GET_CAPS;
	recvlen = sizeof(uint32_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	pcaps->caps = buf_get_u32(devh->in_buffer);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_get_caps_ex(jlink_usb_t *devh, jlink_caps_t *pcaps)
{
	int retv, recvlen;

	if (devh == NULL || pcaps == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_GET_CAPS_EX;
	recvlen = 32*sizeof(uint8_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	pcaps->caps = buf_get_u32(devh->in_buffer);
	memcpy(pcaps->capsex, &devh->in_buffer[4], 32-4);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_get_hw_version(jlink_usb_t *devh, uint32_t *phwver)
{
	int retv, recvlen;

	if (devh == NULL || phwver == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_GET_HW_VERSION;
	recvlen = sizeof(uint32_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	*phwver = buf_get_u32(devh->in_buffer);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_get_unknow_e6(jlink_usb_t *devh, jlink_unknow_e6_t *pe6)
{
	int retv, recvlen;

	if (devh == NULL || pe6 == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_GET_UNKNOW_E6;
	recvlen = sizeof(jlink_unknow_e6_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	memcpy(pe6, devh->in_buffer, retv);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_get_unknow_eb(jlink_usb_t *devh, jlink_unknow_eb_t *peb)
{
	int retv, recvlen;

	if (devh == NULL || peb == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_GET_UNKNOW_EB;
	// eb 6e 04 01  00 00 00 00  00
	devh->out_buffer[1] = 0x6e;
	devh->out_buffer[2] = 0x04;
	devh->out_buffer[3] = 0x01;
	devh->out_buffer[4] = 0x00;
	devh->out_buffer[5] = 0x00;
	devh->out_buffer[6] = 0x00;
	devh->out_buffer[7] = 0x00;
	devh->out_buffer[8] = 0x00;

	recvlen = sizeof(jlink_unknow_eb_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 9, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	memcpy(peb, devh->in_buffer, retv);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_get_unknow_c6(jlink_usb_t *devh, jlink_unknow_c6_t *pc6)
{
	int retv, recvlen;

	if (devh == NULL || pc6 == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_GET_UNKNOW_C6;
	// c6 00 00 00  00
	devh->out_buffer[1] = 0x00;
	devh->out_buffer[2] = 0x00;
	devh->out_buffer[3] = 0x00;
	devh->out_buffer[4] = 0x00;

	recvlen = sizeof(jlink_unknow_c6_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 5, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	memcpy(pc6, devh->in_buffer, retv);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_get_state(jlink_usb_t *devh, jlink_state_t *pstate)
{
	int retv, recvlen;

	if (devh == NULL || pstate == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_GET_STATE;
	recvlen = 8;
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	pstate->voltage = buf_get_u16(devh->in_buffer);
	pstate->tck = devh->in_buffer[2];
	pstate->tdi = devh->in_buffer[3];
	pstate->tdo = devh->in_buffer[4];
	pstate->tms = devh->in_buffer[5];
	pstate->tres = devh->in_buffer[6];
	pstate->trst = devh->in_buffer[7];
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_get_unknow_09(jlink_usb_t *devh, jlink_unknow_09_t *p09, int ab)
{
	int retv, recvlen;

	if (devh == NULL || p09 == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_GET_UNKNOW_09;
	devh->out_buffer[1] = 0x64;		// unknow
	devh->out_buffer[2] = 0xf4;
	devh->out_buffer[3] = 0x05;
	devh->out_buffer[4] = 0x00;
	devh->out_buffer[5] = 0x00;
	devh->out_buffer[6] = 0x00;
	devh->out_buffer[7] = 0x00;
	devh->out_buffer[8] = 0x00;
	devh->out_buffer[9] = 0x00;
	devh->out_buffer[10] = 0x00;
	devh->out_buffer[11] = 0x00;
	devh->out_buffer[12] = (ab==0?0x00:0x04);
	devh->out_buffer[13] = 0x00;

	recvlen = sizeof(jlink_unknow_09_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 14, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	memcpy(p09, devh->in_buffer, retv);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_get_hw_info(jlink_usb_t *devh, jlink_hwinfo_t *phwinfo)
{
	int retv, recvlen;
	uint32_t maskv;

	if (devh == NULL || phwinfo == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_GET_HW_INFO;
	maskv = HW_INFO_POWER_ENABLED | HW_INFO_POWER_OVERCURRENT | HW_INFO_ITARGET | HW_INFO_ITARGET_PEAK;
	buf_set_u32(&devh->out_buffer[1], maskv);
	recvlen = 4 * sizeof(uint32_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1+sizeof(uint32_t), devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	phwinfo->power_enabled = buf_get_u32(devh->in_buffer);
	phwinfo->power_overcurrent = buf_get_u32(&devh->in_buffer[4]);
	phwinfo->itarget = buf_get_u32(&devh->in_buffer[8]);
	phwinfo->itarget_peak = buf_get_u32(&devh->in_buffer[12]);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_get_counters(jlink_usb_t *devh, jlink_counters_t *pcounter)
{
	int retv, recvlen;
	uint32_t maskv;

	if (devh == NULL || pcounter == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_GET_COUNTERS;
	maskv = CNT_INDEX_POWER_ON | CNT_INDEX_POWER_CHANGE;
	buf_set_u32(&devh->out_buffer[1], maskv);
	recvlen = sizeof(jlink_counters_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1+sizeof(uint32_t), devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	pcounter->power_on = buf_get_u32(devh->in_buffer);
	pcounter->power_change = buf_get_u32(&devh->in_buffer[4]);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_measure_rtck_react(jlink_usb_t *devh, jlink_rtck_t *prtck)
{
	int retv, recvlen;

	if (devh == NULL || prtck == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_MEASURE_RTCK_REACT;
	recvlen = sizeof(jlink_rtck_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	prtck->result = buf_get_u32(devh->in_buffer);
	prtck->minimum = buf_get_u32(&devh->in_buffer[4]);
	prtck->maximum = buf_get_u32(&devh->in_buffer[8]);
	prtck->average = buf_get_u32(&devh->in_buffer[12]);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_reset_trst(jlink_usb_t *devh)
{
	return jlinkusb_simple_command(devh, EMU_CMD_RESET_TRST);
}

JLINKUSB_API int jlinkusb_set_speed(jlink_usb_t *devh, uint16_t speed)
{
	int retv, recvlen;

	if (devh == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_SET_SPEED;
	buf_set_u16(&devh->out_buffer[1], speed);
	recvlen = 1 + sizeof(uint16_t);
	retv = jlinkusb_write(devh, devh->out_buffer, recvlen);
	if (retv != recvlen)
	{
		jlinkusb_unlock(devh);
		return -2;
	}

	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_select_if(jlink_usb_t *devh, uint8_t subcmd, uint32_t *presp)
{
	int retv, recvlen;

	if (devh == NULL || presp == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_SELECT_IF;
	devh->out_buffer[1] = subcmd;
	recvlen = sizeof(uint32_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 2, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	*presp = buf_get_u32(devh->in_buffer);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_set_ks_power(jlink_usb_t *devh, uint8_t onoff)
{
	int retv, recvlen;

	if (devh == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_SET_KS_POWER;
	devh->out_buffer[1] = onoff;
	recvlen = 1 + sizeof(uint8_t);
	retv = jlinkusb_write(devh, devh->out_buffer, recvlen);
	if (retv != recvlen)
	{
		jlinkusb_unlock(devh);
		return -2;
	}

	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_hw_clock(jlink_usb_t *devh, uint8_t *ptdi)
{
	int retv, recvlen;

	if (devh == NULL || ptdi == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_HW_CLOCK;
	recvlen = sizeof(uint8_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	*ptdi = devh->in_buffer[0];
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_hw_tms0(jlink_usb_t *devh)
{
	return jlinkusb_simple_command(devh, EMU_CMD_HW_TMS0);
}

JLINKUSB_API int jlinkusb_hw_tms1(jlink_usb_t *devh)
{
	return jlinkusb_simple_command(devh, EMU_CMD_HW_TMS1);
}

JLINKUSB_API int jlinkusb_hw_data0(jlink_usb_t *devh)
{
	return jlinkusb_simple_command(devh, EMU_CMD_HW_DATA0);
}

JLINKUSB_API int jlinkusb_hw_data1(jlink_usb_t *devh)
{
	return jlinkusb_simple_command(devh, EMU_CMD_HW_DATA1);
}

JLINKUSB_API int jlinkusb_hw_trst0(jlink_usb_t *devh)
{
	return jlinkusb_simple_command(devh, EMU_CMD_HW_TRST0);
}

JLINKUSB_API int jlinkusb_hw_trst1(jlink_usb_t *devh)
{
	return jlinkusb_simple_command(devh, EMU_CMD_HW_TRST1);
}

JLINKUSB_API int jlinkusb_hw_jtag3(jlink_usb_t *devh, jlink_jtag_acc_t *pjtagacc)
{
	int retv, numbyte, sendlen, recvlen;

	if (devh == NULL || pjtagacc == NULL)
		return -1;
	if (pjtagacc->tdi == NULL || pjtagacc->tms == NULL || pjtagacc->tdo == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_HW_JTAG3;
	devh->out_buffer[1] = 0x00;
	sendlen = 2;
	buf_set_u16(&devh->out_buffer[sendlen], pjtagacc->numbits);
	sendlen += sizeof(pjtagacc->numbits);
	numbyte = (pjtagacc->numbits + 7) >> 3;
	memcpy(&devh->out_buffer[sendlen], pjtagacc->tms, numbyte);
	sendlen += numbyte;
	memcpy(&devh->out_buffer[sendlen], pjtagacc->tdi, numbyte);
	sendlen += numbyte;

	recvlen = numbyte + sizeof(pjtagacc->result);
	memset(devh->in_buffer, 0, recvlen);

	retv = jlinkusb_access(devh, devh->out_buffer, sendlen, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	memcpy(pjtagacc->tdo, devh->in_buffer, numbyte);
	memcpy(&pjtagacc->result, &devh->in_buffer[numbyte], sizeof(pjtagacc->result));
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_hw_jtag_write(jlink_usb_t *devh, jlink_jtag_acc_t *pjtagacc)
{
	int retv, numbyte, sendlen, recvlen;

	if (devh == NULL || pjtagacc == NULL)
		return -1;
	if (pjtagacc->tdi == NULL || pjtagacc->tms == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_HW_JTAG3;
	devh->out_buffer[1] = 0x00;
	sendlen = 2;
	buf_set_u16(&devh->out_buffer[sendlen], pjtagacc->numbits);
	sendlen += sizeof(pjtagacc->numbits);
	numbyte = (pjtagacc->numbits + 7) >> 3;
	memcpy(&devh->out_buffer[sendlen], pjtagacc->tms, numbyte);
	sendlen += numbyte;
	memcpy(&devh->out_buffer[sendlen], pjtagacc->tdi, numbyte);
	sendlen += numbyte;

	recvlen = sendlen;
	retv = jlinkusb_write(devh, devh->out_buffer, sendlen);
	if (retv != recvlen)	
	{
		jlinkusb_unlock(devh);
		return -2;
	}

	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_hw_jtag_get_result(jlink_usb_t *devh, uint8_t *result)
{
	int retv, recvlen;

	if (devh == NULL || result == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_HW_JTAG_GET_RESULT;
	recvlen = sizeof(uint8_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	*result = devh->in_buffer[0];
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_write_dcc(jlink_usb_t *devh)
{
	return -1;
}


JLINKUSB_API int jlinkusb_reset_target(jlink_usb_t *devh)
{
	return jlinkusb_simple_command(devh, EMU_CMD_RESET_TARGET);
}

JLINKUSB_API int jlinkusb_hw_release_reset_stop_ex(jlink_usb_t *devh, jlink_jtag_acc_t *pjtagacc, 
												uint16_t reps, jlink_criteria_t *pcriteria)
{
	return -1;
}

JLINKUSB_API int jlinkusb_hw_release_reset_stop_timed(jlink_usb_t *devh, jlink_jtag_acc_t *pjtagacc,
													uint16_t timeout, jlink_criteria_t *pcriteria)
{
	return -1;
}

JLINKUSB_API int jlinkusb_hw_reset0(jlink_usb_t *devh)
{
	return jlinkusb_simple_command(devh, EMU_CMD_HW_RESET0);
}

JLINKUSB_API int jlinkusb_hw_reset1(jlink_usb_t *devh)
{
	return jlinkusb_simple_command(devh, EMU_CMD_HW_RESET1);
}

JLINKUSB_API int jlinkusb_get_cpu_caps(jlink_usb_t *devh, jlink_cpu_caps_cmd_t *pcapcmd, uint32_t *pcpucaps)
{
	int retv, recvlen;

	if (devh == NULL || pcapcmd == NULL || pcpucaps)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_EXEC_CPU_CMD;
	devh->out_buffer[1] = pcapcmd->device_family;
	devh->out_buffer[2] = pcapcmd->interf;
	devh->out_buffer[3] = pcapcmd->dumy1;
	devh->out_buffer[4] = pcapcmd->dumy2;
	recvlen = sizeof(uint32_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1+sizeof(jlink_cpu_caps_cmd_t), devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	*pcpucaps = buf_get_u32(devh->in_buffer);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_read_config(jlink_usb_t *devh, jlink_config_t *pconfig)
{
	int retv, recvlen;

	if (devh == NULL || pconfig == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_READ_CONFIG;
	recvlen = sizeof(jlink_config_t);
	memset(devh->in_buffer, 0, recvlen);
	retv = jlinkusb_access(devh, devh->out_buffer, 1, devh->in_buffer, recvlen);
	if (retv != recvlen)
	{
		devh->m_nLastRcvLen = retv;
		jlinkusb_unlock(devh);
		return -2;
	}

	memcpy(pconfig, devh->in_buffer, retv);
	jlinkusb_unlock(devh);
	return 0;
}

JLINKUSB_API int jlinkusb_write_config(jlink_usb_t *devh, jlink_config_t *pconfig)
{
	int retv, recvlen;

	if (devh == NULL || pconfig == NULL)
		return -1;

	jlinkusb_lock(devh);
	devh->out_buffer[0] = EMU_CMD_WRITE_CONFIG;
	recvlen = sizeof(jlink_config_t);
	memcpy(&devh->out_buffer[1], pconfig, recvlen);
	retv = jlinkusb_write(devh, devh->out_buffer, recvlen);
	if (retv != recvlen)
	{
		jlinkusb_unlock(devh);
		return -2;
	}

	jlinkusb_unlock(devh);
	return 0;
}

